#pragma once
#include "can.h"
#include <functional>
using std::map;
using std::string;
using std::vector;
//////////////////
typedef struct
{
    std::atomic<float> rpm;
    std::atomic<float> current;
	std::atomic<float> duty;
	std::atomic<float> angle;
	std::atomic<float> last_angle;
	std::atomic<int> round_cnt;
	std::atomic<float> total_angle;
	std::atomic<float> real_angle;
} u10_Msg;

//////////////////
class Motor{
public:
    uint8_t id;
    
    //xyla
    u10_Msg xyla_u10_Msg;

    Motor(uint8_t id_):id(id_){}
    void modify(const uint8_t *data){
        xyla_u10_Msg.rpm = (float)(((int32_t)data[0] << 24)|((int32_t)data[1] << 16)|((int32_t)data[2] << 8)|(int32_t)data[3])  / 60.0f;
        xyla_u10_Msg.current = (float)(((int16_t)data[4] << 8)|(int16_t)data[5]) / 10.0f;
        xyla_u10_Msg.duty = (float)(((int16_t)data[6] << 8)|(int16_t)data[7]) / 1000.0f;
        // std::cout << "rpm : " << xyla_u10_Msg.rpm.load() << std::endl;
    }
    void modify_ang(const uint8_t *data){
        xyla_u10_Msg.last_angle = xyla_u10_Msg.angle.load();
        xyla_u10_Msg.angle = ((int16_t)data[6] << 8 | (int16_t)data[7])/50.0f;

        if( xyla_u10_Msg.angle - xyla_u10_Msg.last_angle > 180.0f){
            xyla_u10_Msg.round_cnt--;
        }
        else if(xyla_u10_Msg.angle - xyla_u10_Msg.last_angle < -180.0f){
            xyla_u10_Msg.round_cnt++;
        }

        xyla_u10_Msg.total_angle = xyla_u10_Msg.round_cnt * 360 + xyla_u10_Msg.angle;
    }
    void setVel(int vel){set_Vel = vel;}
    int read_setVel()const{return set_Vel.load();}
private:
    std::atomic<int> set_Vel;
};
class VESC_CAN_BUS{
public:
    VESC_CAN_BUS(string can_device, int baudrate = 1000000): io(), t(io, boost::asio::chrono::milliseconds(1))
    {
        can_ = new CAN(can_device, baudrate);

        can_->set_Callback([this](const can_frame &frame)
                        {
                if ((frame.can_id&0xFFFF)<=2310){
                    int id = (frame.can_id&0xFFFF)-2304;
                    if(motor_map.find(id) != motor_map.end()){
                        motor_list[motor_map[id]]->modify(frame.data); 
                    }
                }else {
                    int id = (frame.can_id&0xFFFF)-4096;
                    if(motor_map.find(id) != motor_map.end()){
                        motor_list[motor_map[id]]->modify_ang(frame.data); 
                    }
                }
                    
                    
                });
        running_ = true;
        t.async_wait(std::bind(&VESC_CAN_BUS::send_handler, this, std::placeholders::_1));
        io_thread = std::thread([this]
                                { io.run(); });
    }
    ~VESC_CAN_BUS(){
        running_ = false;
        io.stop();
        io_thread.join();
        for(auto &motor:motor_list){
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            // 设置扩展标识符
            frame.can_id = motor->id | CAN_EFF_FLAG | 3 << 8;
            frame.can_dlc = 4;

            // 拷贝数据到帧中
            frame.data[3] = 0;
            frame.data[2] = 0;
            frame.data[1] = 0;
            frame.data[0] = 0;

            // 发送帧
            can_->send(frame);
        }
        delete can_;
        for(auto &motor:motor_list)
            delete motor;
    }
    void insert(Motor *motor){
        
        motor_list.push_back(motor);
        motor_map[motor->id] = motor_list.size()-1;
    }
    Motor& operator[](int id){
        if(motor_map.find(id) != motor_map.end())
            return *motor_list[motor_map[id]];
        else exit(-1);
    }

private:
    CAN *can_;
    vector<Motor *> motor_list;
    map<int,int> motor_map;
    std::atomic<bool> running_;
    std::thread io_thread;
    boost::asio::io_context io;
    boost::asio::steady_timer t;
    void send_handler(const boost::system::error_code &e){
        int Vel;
        if (!e && running_)
        {
            for(auto &motor:motor_list){
                struct can_frame frame;
                memset(&frame, 0, sizeof(frame));

                // 设置扩展标识符
                frame.can_id = motor->id | CAN_EFF_FLAG | 3 << 8;
                frame.can_dlc = 4;

                // 拷贝数据到帧中
                Vel = motor->read_setVel();
                frame.data[3] = Vel & 0xFF;
                frame.data[2] = (Vel >> 8) & 0xFF;
                frame.data[1] = (Vel >> 16) & 0xFF;
                frame.data[0] = (Vel >> 24) & 0xFF;
                // 发送帧
                can_->send(frame);
                
            }
            t.expires_after(boost::asio::chrono::milliseconds(1));
            t.async_wait(std::bind(&VESC_CAN_BUS::send_handler, this, std::placeholders::_1));
    
        }
        else
        {
            std::cerr << "Timer error: " << e.message() << std::endl;
        }
    }
    Motor* get_motor(uint8_t id){
        return motor_list[motor_map[id]];
    }
};



 